package com.rockwell.sniffyhunter.model;

import java.util.HashMap;
import java.util.Map;

import org.opencv.core.Point;

import android.annotation.SuppressLint;
import android.content.Context;
import android.os.Handler;
import android.preference.PreferenceManager;
import android.util.Log;

import com.rockwell.sniffyhunter.utils.ICollector;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IMazeSolver;
import com.rockwell.sniffyhunter.utils.IRobotCommander;
import com.rockwell.sniffyhunter.utils.IRobotLogger;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.RobotCommandSequencerState;
import com.rockwell.sniffyhunter.utils.UnloadMazeLocatorState;
import com.rockwell.sniffyhunter.utils.UnloaderState;

// enum Robot Operation Status 
enum RobotOpStatus
{
	IDLE,
	INITIALIZED,
	RUNNING,
	PAUSING,
	PAUSED,
}

@SuppressLint("DefaultLocale")
public class Robot {
		
	static final String TAG = "robot";
	
	public static int INTERVAL = 50;
	public static int CMD_TIME_OUT = 30000; // Milliseconds
	public static int PICK_TIME_OUT = 120000; // Milliseconds
	public static int MOTION_SETTLING_TIME = 0; // Milliseconds
	public static int MOTION_SETTLING_TIME2 = 3000; // Milliseconds
	public static int LANE_WIDTH = 300; // mm
	public static int FRONT_SENSOR_THRESHOLD = 100; // mm
	public static int DETECT_FRONT_WALL_INDEX = 10; // mm  << setting page
	public static int FINAL_DETECT_FRONT_WALL_INDEX = 5; // mm  << setting page
	public static int LEFT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 70; // mm  << setting page
	public static int RIGHT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 50; // mm  << setting page
	public static int MOVE_INDEX = 25; // mm
	public static int MOVE_BACK_TO_ENSURE_PICK_AREA_WITHIN_FOV_DIST = 40;//mm
	public static int PICK_TREASURE_FINAL_MOVE = 10; // mm
	public static int UTURN_MOVE_INDEX = 20; //mm  << setting page
	public static int TURN_INDEX = 3; // Degree
	public static int SMALL_TURN_INDEX = 2; // Degree
	public static int SMALL_MOVE_INDEX = 10; // mm
	public static int MOVE_BACK_BEFORE_ARM_DOWN_DIST = 40; // mm
	public static int DETECT_TREASURE_ABSCENT_MOVE_INDEX = 30; // mm
	public static int SIDE_SENSOR_OFFSET = 80; // mm
	public static int DETECT_FRONT_WALL_DISTANCE = 400; // mm
	public static int LEFT_REVERSE_INTO_JUNCTION = 100; // mm
	public static int MAX_PICK_TREASURE_RETRY = 3;
	public static int RIGHT_REVERSE_INTO_JUNCTION = 80; // mm
	public static int ADJUST_ANGLE = 0; // Degree
	public static int COMPENSATION_ANGLE_TOLERANCE = 5; // Degree
	
	private SensorID m_sensorDetectedOpening = SensorID.Max;
	private int m_cmdTimeOut = 0;
	private int m_compensationAngle = 0;
	private int m_pickTreasureRetryCount = 0;
	private RobotPosition m_robotPosition;
	private Point m_treasureDrift;
	private Point m_obstacleDrift;
	private int m_adjustAngle = 0;
	private int m_amountToAdjust = 0;
	private int m_amountToIndex = 0;
	private int m_idealOrientation = 0;
	private int m_curOrientation = 0;
	private int m_turn_direction = 1;
	private int m_turn_angle = 0;
	private int m_turn_count = 1;
	public int m_logLevel = 1;
	private Object m_pauseResumeLock = new Object();
	private boolean stepMode = false;
	private boolean useCompassCompensation = false;
	private boolean useCompassCompensationDuringTurn = false;
	private StatefulRobot m_statefulRobot;
	private RobotCommandSequencer m_commandSequencer;
	private RobotOpStatus m_robotOpStatus = RobotOpStatus.IDLE;
	public NorthernStarFinder m_northernStarFinder;
	
	private boolean m_needSendJogCommandBeforeStart = false;
	public RobotCommand m_lastCommand = new RobotCommand();

	private double m_robotWidth = 15; // 15cm
	private double m_robotLength = 15; // 15cm
	
	private boolean m_gameOver = false;
	
	private Handler m_missionHandler = new Handler();
	private Runnable m_mission = new Runnable() {

		@Override
		public void run() {
			log("Robot Started", 1);
			while(true) {
				try {
						if (m_robotOpStatus == RobotOpStatus.IDLE) {
							initialize();
						}
						
						if (m_robotOpStatus == RobotOpStatus.INITIALIZED || 
								m_robotOpStatus == RobotOpStatus.PAUSING) {
							runBeforePause();
							waitForResume();
							runBeforeStart();
						}
						m_statefulRobot.PerformAction();
						sleep(5);
					}
				catch (Exception ex) {
					log(ex, 1);
					runBeforePause();
					waitForResume();
					runBeforeStart();
				}
			}
		}
		
	};

	private Thread m_backgroundThread = new Thread(m_mission);
	
	public IRobotSensors m_sensors;
	public IRobotCommander m_commander;
	private Maze m_maze;
	public IMazeSolver m_solver;
	public IRobotPositionDetector m_positionDetector;
	public Map<Integer, IDetector> m_colorDetectors;
	public ICollector m_treasureCollector;
	
	private boolean m_usePredefinedPath;
	public String m_easySolver;
	public String m_mediumSolver;
	public String m_hardSolver;
	
	// avoid creating lots of RobotCommand objects
	private RobotCommand m_command = new RobotCommand();
	
	public IRobotLogger m_logger;

	
	public int getTreasurePickCount() {
		return m_treasureCollector.getItemCount();
	}
	
	public void increaseTreasurePickCount() {
		m_treasureCollector.increaseCount();
	}
	
	public void resetTreasurePickCount() {
		m_treasureCollector.resetItemCount();
	}
	
	public boolean isBasketFull() {
		return m_treasureCollector.isFull();
	}
	
	public Robot(double width, double length, 
			int startZone,
			IRobotSensors sensors, 
			IRobotCommander commander, 
			Maze maze, 
			IMazeSolver solver, 
			Map<Integer, IDetector> colorDetectors,
			ICollector treasureCollector) {
		m_sensors = sensors;
		m_positionDetector = new RobotPositionDetector(sensors);
		
		m_colorDetectors = colorDetectors;
		m_commander = commander;
		m_maze = maze;
		m_solver = solver;
		m_statefulRobot = new StatefulRobot(this);
		m_commandSequencer = new RobotCommandSequencer(this);
		m_northernStarFinder = new NorthernStarFinder(startZone);
		m_treasureCollector = treasureCollector;
	}

	public void PopulatePredefinedSolvers(Context context) {
		m_usePredefinedPath = PreferenceManager.getDefaultSharedPreferences(context).getBoolean("use_predefined_path", false);
		m_easySolver = PreferenceManager.getDefaultSharedPreferences(context).getString("predefined_path_easy", "");
		m_mediumSolver = PreferenceManager.getDefaultSharedPreferences(context).getString("predefined_path_medium", "");
		m_hardSolver = PreferenceManager.getDefaultSharedPreferences(context).getString("predefined_path_hard", "");
	}

	public void setLogger(IRobotLogger logger, int level) {
		m_logger = logger;
		m_logLevel = level;
	}
	
	public void log(String msg, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(msg);
			
			if ((stepMode && level <= 2) || level == 0) {
				runBeforePause();
				waitForResume();
				runBeforeStart();
			}
		}
	}
	
	private void logSensors() {
		String msg;
		msg = String.format("F[%s],L[%s],R[%s]", m_robotPosition.frontwalldetected? "Y":"N",
								m_robotPosition.leftwalldetected? "Y":"N",
								m_robotPosition.rightwalldetected? "Y":"N");
		log(msg, 2);
	}
	
	public void log(Exception ex, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(ex);
		}
	}

	public void changeSolver(IMazeSolver solver) {
		m_solver = solver;
	}
	
	public double getWidth() {
		return m_robotWidth;
	}
	
	public void setWidth(double width) {
		m_robotWidth = width;
	}
	
	public double getLength() {
		return m_robotLength;
	}
	
	public void setLength(double length) {
		m_robotLength = length;
	}
	
	public void setOrientation(float angle) {
		m_sensors.setOrientation(angle);
	}
	
	public void onDetect(int index, boolean present, Point center, float radius, int length, int width) {
		m_colorDetectors.get(index).onDetect(present, center, radius, length, width);
	}

	public void useStepMode(boolean use) {
		this.stepMode = use;
	}
	
	/**
	 * The location of the sensor from the left edge of the robot.
	 * @return
	 */
	public double getLeftDistanceSensorLocation() {
		return 2.5;	
	}
	
	/**
	 * The location of the sensor from the front of the robot.
	 * @return
	 */
	public double getFronDistanceSensorLocation() {
		return 1.5;
	}
	
	public void setStepMode(boolean step) {
		stepMode = step;
	}
	
	public void setUseCompassCompensation(boolean use) {
		useCompassCompensation = use;
	}
	
	public void setUseCompassCompensationDuringTurn(boolean use) {
		useCompassCompensationDuringTurn = use;
	}
	
	public boolean isUseCompassCompensationDuringTurn() {
		return useCompassCompensationDuringTurn;
	}
	
	public void start(boolean startThread) throws Exception {
		m_gameOver = false;

		m_commander.connect();
		
		if (m_commander.isConnected()) {
			
			if (m_logger != null)
				m_logger.log("Modbus connected");
			
			//m_missionHandler.postDelayed(m_mission, INTERVAL);
			if(startThread) {
				m_backgroundThread.start();
				
				if (m_logger != null)
					m_logger.log("thread started");
			}
			
			Log.i(TAG, "started");
		}
		else {
			throw new Exception("Cannot establish Modbus connection.");
		}
	}

	public void quit() {
		m_gameOver = true;
		
		Log.i(TAG, "quit");
	}
	
	public void initialize() {
		reconnectModbus();
		m_statefulRobot.Initialize();
		m_commandSequencer.Initialize();
		
		/*while (performSendCommand(RobotAction.Jog, m_idealOrientation) != 1) {
			if (sendCommandSequence == SendCommandEnum.ERROR) {
				sendCommandSequence = SendCommandEnum.SEND;
				break;
			}
		};
		*/

		sleep(2000);

		m_commander.reset();
		m_needSendJogCommandBeforeStart = false;
		m_idealOrientation = (int)m_sensors.getOrientation();

		String msg = String.format("Initial Orientation[%d] @ continueMission", m_idealOrientation);
		log(msg, 2);
		
		m_robotOpStatus = RobotOpStatus.INITIALIZED;
	}
	
	public void restart() {
		synchronized (m_pauseResumeLock) {
			m_robotOpStatus = RobotOpStatus.IDLE;
			m_pauseResumeLock.notify();
		}
	}
	
	public boolean isPaused() {
		return (m_robotOpStatus == RobotOpStatus.PAUSED);
	}
	
	public void resumeRun() {
		synchronized (m_pauseResumeLock) {
			m_pauseResumeLock.notify();
	  	}
	}
	
	public void pause() {
		synchronized (m_pauseResumeLock) {
			if (m_robotOpStatus != RobotOpStatus.PAUSED)
				m_robotOpStatus = RobotOpStatus.PAUSING;
	  	}
	}
	
	private void waitForResume() {

		synchronized (m_pauseResumeLock) {
			try {
				if (m_robotOpStatus == RobotOpStatus.IDLE)
					return;
				
				m_robotOpStatus = RobotOpStatus.PAUSED;
				if (m_logger != null)
					m_logger.onRobotPaused();
				
				m_pauseResumeLock.wait();
				
				if (m_robotOpStatus != RobotOpStatus.IDLE) {
					m_robotOpStatus = RobotOpStatus.RUNNING;
					if (m_logger != null)
						m_logger.onRobotResumed();
				}
				else {
					if (m_logger != null)
						m_logger.onRobotIdle();
				}

			}
			catch(InterruptedException e) { 
				Log.i(TAG, "resume error"); 
			} 
		}
	}
	
	private void runBeforeStart() {
		
		if (m_needSendJogCommandBeforeStart) {
			
			ReturnResult returnVal;
			m_commandSequencer.Initialize();
			 
			while (true) {
				
				returnVal = m_commandSequencer.PerformAction(m_lastCommand.action, m_lastCommand.param1);
				
				if (returnVal == ReturnResult.Completed) {
					log("jog.runBeforeStart", 4);
					break;
				}
			};
			
			m_needSendJogCommandBeforeStart = false;
		}
	}
	
	private void runBeforePause() {
		
		m_lastCommand.action = m_commander.m_lastCommand.action;
		m_lastCommand.param1 = m_commander.m_lastCommand.param1;
		
		 if (m_commander.isJogging()) {
			 
			ReturnResult returnVal;
			m_commandSequencer.Initialize();
			
			while (true) {
				
				returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);
				
				if (returnVal == ReturnResult.Completed) {
					log("stop jog.runBeforePause", 4);
					break;
				}
			};
			
			m_needSendJogCommandBeforeStart = true;
		}
	}
	
	private void reconnectModbus() {
		try {
			m_commander.disconnect();
			m_commander.connect();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	private void sleep(int duration) {
		try {
			Thread.sleep(duration);
		} catch (InterruptedException ex) {
			log(ex, 1);
		}
	}
	
	public void testStatefulRobot() {
		
	}
	
	public void testJog() {
		RobotCommandSequencer sequencer = new RobotCommandSequencer(this);
		while (sequencer.PerformAction(RobotAction.Jog) != ReturnResult.Completed);
	}

	public void testJog(int jogCycle) {

	}
	
	private void testSendCommand(RobotAction action) {
		RobotCommandSequencer sequencer = new RobotCommandSequencer(this);
		while (sequencer.PerformAction(action) != ReturnResult.Completed) {
			if (sequencer.getStateMachine() == RobotCommandSequencerState.ERROR)
				break;
		}
	}
	
	private void testSendCommand(RobotAction action, int param) {
		RobotCommandSequencer sequencer = new RobotCommandSequencer(this);
		while (sequencer.PerformAction(action, param) != ReturnResult.Completed) {
			if (sequencer.getStateMachine() == RobotCommandSequencerState.ERROR)
				break;
		}
	}

	@SuppressLint("UseSparseArrays") @SuppressWarnings("serial")
	public void testRobotAction(int robotActionIndex, int parameter) {
		Map<Integer, RobotAction> robotActionTable = new HashMap<Integer, RobotAction>() {{
			put(0, RobotAction.Jog);
			put(1, RobotAction.TurnDegree);
			put(2, RobotAction.Move);
			put(3, RobotAction.Move2);
			put(4, RobotAction.PickTreasure);
			put(5, RobotAction.ArmDown);
			put(6, RobotAction.Unload);
			put(7, RobotAction.Stop);
		};};
		
		RobotAction robotAction = robotActionTable.get(robotActionIndex);
		
		if (robotAction == RobotAction.TurnDegree || 
			robotAction == RobotAction.Move || 
			robotAction == RobotAction.Move2 ||
			robotAction == RobotAction.Jog
			)
			testSendCommand(robotAction, parameter);
		else
			testSendCommand(robotAction);
	}
	
	private enum TestSequenceEnum {
/*
		<string-array name="robot_sequences">
	        <item>1. Sniff</item>
	        <item>2. Move to Treasure</item>
			<item>3. Pick Treasure</item>
			<item>4. Move and Pick Treasure</item>
			<item>5. Move back to sniff</item>
			<item>6. Move to Next Zone</item>
			<item>7. Skip Obstacle</item>
			<item>8. Unload Treasure</item>
			<item>9. Turn Left in Junction</item>
			<item>10. Turn Right in Junction</item>
			<item>11. U Turn in Junction</item>
			<item>12. Locate Maze</item>
			<item>13. Align Maze Wall</item>
			<item>14. Locate Maze Entrance</item>
			<item>15. Navigate in Maze</item>
			<item>16. Stateful Robot</item>
			<item>17. Unloader</item>
			<item>18. Align Maze Wall ++</item>
			<item>19. Locate Maze Entrance ++</item>
		</string-array>
*/
		START,
		SNIFF,
		MOVE_TO_TREASURE,
		PICK_TREASURE,
		MOVE_PICK_TREASURE,
		MOVE_BACK_TO_SNIFF,
		MOVE_TO_NEXT_ZONE,
		SKIP_OBSTACLE,
		UNLOAD_TREASURE,
		TURN_LEFT_IN_JUNCTION,
		TURN_RIGHT_IN_JUNCTION,
		U_TURN_IN_JUNCTION,
		LOCATE_MAZE,
		ALIGN_MAZE_WALL,
		LOCATE_MAZE_ENTRANCE,
		NAVIGATE_IN_MAZE,
		STATEFUL_ROBOT,
		UNLOADER,
		ALIGN_MAZE_WALL_PLUS,
		LOCATE_MAZE_ENTRANCE_PLUS;
		
		private static TestSequenceEnum[] sequences = values();
	    public static TestSequenceEnum get(int n) {return sequences[n];}
	}
	
	public void testRobotSequence(int robotSequenceIndex) {

		StateObject stateObject = null;
		
		switch (TestSequenceEnum.get(robotSequenceIndex)) {
		case SNIFF:
		{
			stateObject = new SnifferObject(this);
			break;
		}
		case MOVE_TO_TREASURE:
		{
			stateObject = new MoveToTreasure(this);
			break;
		}
		case PICK_TREASURE:
		{
			stateObject = new PickTreasure(this);
			break;
		}
		case MOVE_PICK_TREASURE:
		{
			stateObject = new Picker(this);
			break;
		}
		case MOVE_BACK_TO_SNIFF:
		{
			stateObject = new MoveBackToSniff(this);
			break;
		}
		case MOVE_TO_NEXT_ZONE:
		{
			stateObject = new MoveToNextZoneObject(this);
			break;
		}
		case SKIP_OBSTACLE:
		{
			stateObject = new ObstacleSkipper(this);
			break;
		}
		case UNLOAD_TREASURE:
		{
			stateObject = new TreasureUnloader(this);
			break;
		}
		case TURN_LEFT_IN_JUNCTION:
		{
			RobotCommand command = new RobotCommand(RobotAction.TurnLeft);
			stateObject = new Turner(this, command);
			break;
		}
		case TURN_RIGHT_IN_JUNCTION:
		{
			RobotCommand command = new RobotCommand(RobotAction.TurnRight);
			stateObject = new Turner(this, command);
			break;
		}
		case U_TURN_IN_JUNCTION:
		{
			RobotCommand command = new RobotCommand(RobotAction.TurnBack);
			stateObject = new Turner(this, command);
			break;
		}
		case LOCATE_MAZE:
		{
			stateObject = new UnloadMazeLocator(this);
			break;
		}
		case ALIGN_MAZE_WALL:
		{
			stateObject = new MazeWallAligner(this);
			break;
		}
		case LOCATE_MAZE_ENTRANCE:
		{
			stateObject = new EntranceLocator(this);
			break;
		}
		case NAVIGATE_IN_MAZE:
		{
			stateObject = new UnloadMazeNavigator(this);
			break;
		}
		case STATEFUL_ROBOT:
		{
			stateObject = new StatefulRobot(this);
			break;
		}
		case UNLOADER:
		{
			stateObject = new Unloader(this);
			break;
		}
		case ALIGN_MAZE_WALL_PLUS:
		{
			stateObject = new Unloader(this);
			stateObject.machineState = UnloaderState.ALIGN_ROBOT_WITH_MAZE;
			break;
		}
		case LOCATE_MAZE_ENTRANCE_PLUS:
		{
			stateObject = new Unloader(this);
			stateObject.machineState = UnloaderState.LOCATE_ENTRANCE;
			break;
		}
		default:
		{
			break;
		}}
		
		if (stateObject != null) {
			while (stateObject.PerformAction() != ReturnResult.Completed);
		}
	}

	public boolean shouldUsePredefined() {
		return m_usePredefinedPath;
	}
}
